-- 当前为机器人自动模式代码,实现自动动作功能
function AutoMotion()
	
	-- 是否执行复位完成标志位
	ReturnHomeLab = false
	
	-- 定义机器人动作逻辑状态
	RbtPick1 = 1
	RbtPick2 = 2
	RbtPlace = 3

	-- 定义打开气缸的次数
	
	GrippernNUM = 0
	
	-- 定义机器人动作逻辑状态变量
	RbtMotionStt =  0
	
	-- 描述: 机器人复位逻辑.根据实际情况,进行复位动作.
	-- 输入: 无
	-- 输出: 无
	function ReturnHome() 
		IO.Set(DO_CHECK_CP_PLC,0)	--清除有卡簧的信号
		IO.Set(DO_HOMEFINISH,0)		--清除复位完成信号
		Thread.Sleep(1)
	
		Stt = 0
		-- 上伺服 & 回零
		Bool_power,Bool_home = Robot.State(1)
		if Bool_power == 1 then
			RbtPowerOn = true
		else
			RbtPowerOn = false
		end
    
		if RbtPowerOn == false then
			-- 机器人上伺服
			Robot.PowerEnable(1,1)
		end

		-- 机器人回零
		Robot.Home(1)
		
		--设置安全速度
		Safeprof = Profile.New(10,0.000,30,30,0.1,0.1,-1)
		Safeprof:Set(1)
		
		-- 回零动作
		mHereCheckLoc = Location.New()
		mHereCheckLoc:Here(1)
		
		-- 程序回零需要考虑手系和机器人的位置
		-- 【1】机器人如果在抓取点区域，可以考虑直接回安全点
		-- 【2】机器人如果在放置点区域，首先判读机器人的位置
		if (mHereCheckLoc.Config ~= 2) then
			print("机器人手系姿态不正确")
			GDouble[3]=-20004
			IO.Set(DO_ALARM,1)
			Stt = -1
			return Stt
		else	
			-- 移动到安全高度，这个函数会先判断机器人机器人的安全区域
			Stt = MoveToSafeHeight()
			if(Stt ~= 0 ) then
				print("关闭程序")
				return Stt
			end

			-- 移动到安全点
			Move.Line(SafeLoc)
			Move.WaitForEOM()
			print("机器人已经回到安全点")
			
			-- 检测是否有卡簧信号，然后去对应的点位
			if (IO.Get(DI_CHECK_CP) == 1) then
				RbtMotionStt = RbtPlace
				-- 给PLC一个有卡簧的信号
				IO.Set(DO_CHECK_CP_PLC,1)
				--Move.Line(PassLoc2)	--问题：复位不能运动到取/放等待点
				--Move.WaitForEOM()
				print("机器人已经回到等待打卡过渡点")
			else
				RbtMotionStt = RbtPick
				--Move.Line(PickWUpLoc1)
				--Move.WaitForEOM()
				print("机器人已经回到安全位置等待取料信号")
			end
			
			Thread.Sleep(0.2)
			
			IO.Set(DO_HOMEFINISH,1)
		end
		return Stt
	end
	
	-- 描述: 实时设置机器人速度参数
	-- 输入: 无
	-- 输出: 无
	function SetSpeed() 
		
		ReadMoveProf = GDouble[27]--横移速度
		if ReadMoveProf > 100 then
			ReadMoveProf = 100
		end 
		if ReadMoveProf < 1 then
			ReadMoveProf = 1
		end
		
		if MoveProf.Speed ~= ReadMoveProf then
			MoveProf = Profile.New(ReadMoveProf,0.000,80,80,0.1,0.1,-1)
			MoveProf:Set(1)
			print("MoveProf:"..ReadMoveProf)
		end
		
		ReadDownProf = GDouble[28]--上下
		
		if ReadDownProf > 100 then
			ReadDownProf = 100
		end 
		if ReadDownProf < 1 then
			ReadDownProf = 1
		end 
		
		if DownProf.Speed ~= ReadDownProf then
			DownProf = Profile.New(ReadDownProf,0.000,ReadDownProf,ReadDownProf,0.050,0.050,-1)
			print("DownProf:"..ReadDownProf)
		end
	
	end
	
	
	print("AutoMotion Thread Start!!!")
	
	-- 自动模块线程启动入口
	try{
		function()
			print("137-auto r----------------+-+-+-+-+-+-+-+-+-+-+")
			Robot.Attached(1)
			
			-- 切换正确坐标系
			Robot.Frame(1,2)
			
			TimeNum = 0
			-- 触发PLC交互线程获取PLC点位
			GInt[10]=1
			-- 等待获取PLC点位完毕
			while GInt[10] ==  1 do
				--超时处理
				Thread.Sleep(0.05)
				TimeNum = TimeNum + 1
				if TimeNum > 100 then
					print("152--------超时:获取点位信息错误")
					-- 报警
					GDouble[3]=-20000
					IO.Set(DO_ALARM,1)
				end
			end
			
			if GInt[10] == 0 then
				-- 根据获取的PLC点位进行相关的赋值
				SetLoc()
			else
				print("163-------自动线程:获取点位信息错误")
				goto Wrong
			end
			
			while true do
			
				-- 执行主程序
				if (IO.Get(DI_MAINSTRAT) ==  1) then
					
					print("172------执行主程序")
				
					if ReturnHomeLab == false then
						
						print("176-------执行主程序:准备复位动作")
					
						-- 复位动作,并检测有无料,判断机器人的位置
						Stt = ReturnHome()
						if Stt ~=0 then
							break
						end 
						ReturnHomeLab = true
						
						print("185-----执行主程序:复位动作执行完成")
						
					end
			
					while true do
						
						-- 实时设置机器人速度参数
						SetSpeed()
						
						-- 机器人抓取
						if (RbtMotionStt ==  RbtPick)and  (ReturnHomeLab ==  true)then
							print("---196-------")
							-- 抓取点1
							if (IO.Get(DI_PICK1) == 1) then
								
								IO.Set(DO_HOMEFINISH,0)
								print("201--------准备去抓取点1")
								
								Move.Line(PickWUpLoc1)
								Move.WaitForEOM()
								print("205-----抓取等待点1上方")
								
								Move.Line(PickWDownLoc1)
								Move.WaitForEOM()
								print("抓取等待点1下方")
								
								Move.Line(PickLoc1)
								Move.WaitForEOM()
								print("到达抓取点1")
								
								Thread.Sleep(0.2)
								
								-- 抓取完成之后需要抬起来去放置点等待
								
								-- 去打抓取点1 上方
								PickLoc1Clone = Location.New()
								PickLoc1Clone = PickLoc1:Clone()
								PickLoc1Clone.Z =  -72
								
								Move.Line(PickLoc1Clone)
								Move.WaitForEOM()
								
								SafeProf:Set(1)
								-- 过渡点1
								Move.Line(PassLoc1)
								Move.WaitForEOM()
								print("抓取过渡点")
								IO.Pulse(DO_MOTIONFINISH,100)
								
								-- 过渡点2
								Move.Line(PassLoc2)
								Move.WaitForEOM()
								print("236等待打卡过渡点")
		
								
								RbtMotionStt = RbtPlace
							
							-- 抓取点2
							elseif (IO.Get(DI_PICK2) == 1) then
								
								IO.Set(DO_HOMEFINISH,0)
								print("准备去抓取点2")

								--Move.Line(PickBackUpLoc2)--变量名错误
								Move.Line(PickWUpLoc2)
								Move.WaitForEOM()
								print("到达抓取点2后上方")
								
								--Move.Line(PickBackLoc2)--变量名错误
								Move.Line(PickWDownLoc2)
								Move.WaitForEOM()
								print("到达抓取点2后方")
								
								Move.Line(PickLoc2)
								Move.WaitForEOM()
								print("到达抓取点2")
								
								
								-- 去打抓取点1 上方
								PickLoc2Clone = Location.New()
								PickLoc2Clone = PickLoc2:Clone()
								PickLoc2Clone.Z =  -72
								
								
								Move.Line(PickLoc2Clone)
								Move.WaitForEOM()
								
								-- 抓取完成之后需要抬起来去放置点等待
								
								SafeProf:Set(1)
								-- 过渡点1
								Move.Line(PassLoc1)
								Move.WaitForEOM()
								print("抓取过渡点")
								IO.Pulse(DO_MOTIONFINISH,100)
								
								-- 过渡点2
								Move.Line(PassLoc2)
								Move.WaitForEOM()
								print("282等待打卡过渡点")
								
								
								RbtMotionStt = RbtPlace
								
							end
						
						-- 机器人放置	
						elseif (RbtMotionStt ==  RbtPlace)and  (ReturnHomeLab ==  true) then
							-- 过渡点2
							Move.Line(PassLoc2)
							Move.WaitForEOM()
							MoveProf:Set(1)
							--print("294等待打卡过渡点")
						
							-- 放置点
							if (IO.Get(DI_PLACE) == 1) then
							
								IO.Set(DO_HOMEFINISH,0)
								print("准备去放置点打卡簧")
								
								-- 读取PLC 允许读取高度的信号
								while GBool[25] == false do
									Thread.Sleep(0.05)
								end
								print("收到允许读取偏移高度点位信号")
								-- 过渡点3
								Move.Line(PassLoc3)
								Move.WaitForEOM()
								-- 关闭给PLC一个有卡簧的信号
								IO.Set(DO_CHECK_CP_PLC,0)
								
								-- 打卡等待点
								Move.Line(PlaceWait)
								Move.WaitForEOM()
								print("到达打开等待点")
								
								
								-- 去打卡簧中心位置
								PlaceMiddleClone = Location.New()
								PlaceMiddleClone = PlaceMiddle:Clone()
								PlaceMiddleClone.Z = PlaceMiddleClone.Z + GDouble[28]
								Move.Line(PlaceMiddleClone)
								Move.WaitForEOM()
								print("到达打卡中心位置")
								
								-- 去打卡簧偏移位置
								PlaceOffectClone = Location.New()
								PlaceOffectClone = PlaceOffect:Clone()
								PlaceOffectClone.Z = PlaceOffectClone.Z  + GDouble[28]
								
								Move.Line(PlaceOffectClone)
								Move.WaitForEOM()
								print("到达打卡偏移位置")
				
								-- 开始打卡簧
								::Gripper::
									-- 给PLC一个气缸伸出来的动作
									
									
									-- 延时等待
									IO.Set(DO_BACK,0)
									IO.Set(DO_FORWARD,1)
									Thread.Sleep(0.2)
									IO.Set(DO_FORWARD,0)
									Thread.Sleep(0.2)
									
									-- 判断打卡簧伸缩气缸伸出检测
									if  (IO.Get(DI_REACH) == 1) then 
									
										print("打卡簧伸缩气缸伸出到位")
										-- 恢复标志位
										GrippernNUM = 0

										-- 给PLC一个气缸退回去的动作
										-- IO.Pulse(DO_BACK,100)
										IO.Set(DO_FORWARD,0)
										IO.Set(DO_BACK,1)
										Thread.Sleep(0.2)
										IO.Set(DO_BACK,0)
									
										-- 延时等待
										Thread.Sleep(0.2)
										
										-- 判断打卡簧伸缩气缸退回检测
										if  (IO.Get(DI_RETRACT) == 1) then 
										
											-- 抬起来
											Move.Line(PlaceUp)
											Move.WaitForEOM()
											print("到达打卡抬升点")
											
											-- 抬起来做偏移
											Move.Line(PlaceUpOffect)
											Move.WaitForEOM()
											print("到达打卡抬升点偏移")
											
											-- 打卡抬升点左侧偏移
											Move.Line(PlaceUpLeft)
											Move.WaitForEOM()
											print("到达打卡抬升点左侧偏移")
											
											Move.Line(PassLoc3)
											Move.WaitForEOM()
											print("到达打卡位过渡点位置")
											
											Move.Line(PassLoc2)
											Move.WaitForEOM()
											print("到达等待打卡过渡点位置")
											
											-- 给PLC完成信号
											IO.Pulse(DO_MOTIONFINISH,100)
											
											Move.Line(PassLoc1)
											Move.WaitForEOM()
											print("到达抓取过渡点位置")
											
											-- 回到打卡点
											Move.Line(PickWUpLoc1)
											Move.WaitForEOM()
											print("到达1号打卡上方位置")
											
											RbtMotionStt = RbtPick
											
										else
											print("打卡簧伸缩气缸未退回到位")
											GDouble[3]=-20006
											IO.Set(DO_ALARM,1)
											goto Wrong
										end 		
									
									-- 如果气缸没有到位，会再次伸缩气缸打入，只允许打入2次
									else
										
										if (GrippernNUM == 0 )then
											GrippernNUM = GrippernNUM+1
											
											-- 气缸缩回去
											IO.Pulse(DO_BACK,100)
											print("准备第二次打卡簧")
											-- 回去重新打
											goto Gripper
											
										elseif (GrippernNUM == 1 ) then
											print("打卡簧伸缩气缸未到位")
											-- 程序自动停止
											goto Wrong
										end
									end 		 			
							end
							Thread.Sleep(0.05)
							
						end
						
					end
				
				-- 执行复位程序
				elseif (IO.Get(DI_RESETSTART) == 1) then
					
					if ReturnHomeLab == false then
						
						print("执行复位程序:复位动作执行")
					
						-- 复位动作
						Stt = ReturnHome()
						if Stt ~=0 then
							break
						end 
						ReturnHomeLab = true
						
						print("执行复位程序:复位动作执行完成")
						
					end
				
				end
				
				
				Thread.Sleep(0.05)
			end
			
			::Wrong::
			print("::Wrong::程序错误")
			
		end,
		
		catch = function(errors)
			
			-- 打印报错模块名称
			print("AutoMotion.Error")

			-- 打印报错代码所在文件名称
			print("file   :", errors.file)
			-- 打印报错代码行数
			print("line   :", errors.line)
			-- 打印报错信息
			print("message:", errors.message)
			-- 打印报错代码
			print("code   :", errors.code)
			-- 打印报错机器人
			print("robot  :", errors.robot)
			-- 打印报错轴数
			print("axis   :", errors.axis)
			
			ErrorCode = errors.code
			
			if ErrorCode ~= nil then
				ErrorCode = tonumber(errors.code)
			else
				ErrorCode = -20003
			end
			
			-- 超出限位的报错:
			-- 正方向超出限位
			if (ErrorCode == -5031) then
				ErrorAxis =  tonumber(errors.axis)
				if (ErrorAxis == 1) then
					ErrorCode = -20008
				elseif (ErrorAxis == 2) then
					ErrorCode = -20010
				elseif (ErrorAxis == 3) then
					ErrorCode = -20012
				elseif (ErrorAxis == 4) then
					ErrorCode = -20014
				end
			-- 负方向超出限位
			elseif (ErrorCode == -5032) then
				ErrorAxis =  tonumber(errors.axis)
				if (ErrorAxis == 1) then
					ErrorCode = -20009
				elseif (ErrorAxis == 2) then
					ErrorCode = -20011
				elseif (ErrorAxis == 3) then
					ErrorCode = -20013
				elseif (ErrorAxis == 4) then
					ErrorCode = -20015
				end
			end
			
			
			IO.Set(DO_HOMEFINISH,0)	--回零完成信号 置0
			
			GDouble[3]=ErrorCode
			IO.Set(DO_ALARM,1)

		end,
			finally = function(ok,errors)
		
	end
		}
		
end